/*
 * Algorithm.c
 *
 *  Created on: 2023年12月1日
 *      Author: a
 */
#include "Algorithm.h"



//-------------------------------------------------------------------------------------------------------------------
// 函数简介     算法参数初始化函数
// 参数说明     无
// 返回参数     无
// 使用示例     Parameter_Init();
// 备注信息     对各种 PID 参数以及其他相关参数进行初始化设置
//-------------------------------------------------------------------------------------------------------------------
void Parameter_Init(void)
{

	
}

                        /*******滑动滤波器相关********/
                        /*******滑动滤波器相关********/
                        /*******滑动滤波器相关********/

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     滑动平均滤波计算函数（整数类型）
// 参数说明     move_filter：滑动滤波器结构体指针；new_data：新的数据
// 返回参数     无
// 使用示例     move_filter_calc(&filter, data);
// 备注信息     对整数类型数据进行滑动平均滤波，存储最近的 n 个数据并计算平均值
//-------------------------------------------------------------------------------------------------------------------
void move_filter_calc(move_filter_struct *move_filter, signed int new_data)
{
    //加上新的数值，减去最末尾的数值，求得最新的和
    move_filter->data_sum = move_filter->data_sum + new_data - move_filter->data_buffer[move_filter->index];
    //重新求平均值
    move_filter->data_average = move_filter->data_sum / move_filter->buffer_size;

    //将数据写入缓冲区
    move_filter->data_buffer[move_filter->index] = new_data;
    move_filter->index++;
    if(move_filter->buffer_size <= move_filter->index)
    {
        move_filter->index = 0;
    }
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     滑动平均滤波计算函数（双精度类型）
// 参数说明     move_filter：滑动滤波器结构体指针；new_data：新的数据
// 返回参数     无
// 使用示例     move_filter_double_calc(&filter, data);
// 备注信息     对双精度类型数据进行滑动平均滤波，存储最近的 n 个数据并计算平均值
//-------------------------------------------------------------------------------------------------------------------
void move_filter_double_calc(move_filter_double_struct *move_filter, double new_data)
{
    //加上新的数值，减去最末尾的数值，求得最新的和
    move_filter->data_sum = move_filter->data_sum + new_data - move_filter->data_buffer[move_filter->index];
    //重新求平均值
    move_filter->data_average = move_filter->data_sum / move_filter->buffer_size;

    //将数据写入缓冲区
    move_filter->data_buffer[move_filter->index] = new_data;
    move_filter->index++;
    if(move_filter->buffer_size <= move_filter->index)
    {
        move_filter->index = 0;
    }
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     滑动平均滤波初始化函数（整数类型）
// 参数说明     move_filter：滑动滤波器结构体指针
// 返回参数     无
// 使用示例     move_filter_init(&filter);
// 备注信息     初始化整数类型的滑动滤波器结构体，设置缓冲区大小并将缓冲区初始化为 0
//-------------------------------------------------------------------------------------------------------------------
void move_filter_init(move_filter_struct *move_filter)
{
    move_filter->data_average = 0;
    move_filter->data_sum = 0;
    move_filter->index = 0;
    //设置缓冲区大小
    move_filter->buffer_size = MOVE_AVERAGE_SIZE;

    unsigned char i;
    for(i=0; i < move_filter->buffer_size; i++)
    {
        move_filter->data_buffer[i] = 0;
    }
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     滑动平均滤波初始化函数（双精度类型）
// 参数说明     move_filter：滑动滤波器结构体指针
// 返回参数     无
// 使用示例     move_filter_double_init(&filter);
// 备注信息     初始化双精度类型的滑动滤波器结构体，设置缓冲区大小并将缓冲区初始化为 0
//-------------------------------------------------------------------------------------------------------------------
void move_filter_double_init(move_filter_double_struct *move_filter)
{
    move_filter->data_average = 0;
    move_filter->data_sum = 0;
    move_filter->index = 0;
    //设置缓冲区大小
    move_filter->buffer_size = MOVE_AVERAGE_SIZE;

    unsigned char i;
    for(i=0; i < move_filter->buffer_size; i++)
    {
        move_filter->data_buffer[i] = 0;
    }
}
//-------------------------------------------------------------------------------------------------------------------
// 函数简介     一维卡尔曼滤波
// 参数说明    
// 返回参数     无
// 使用示例     void Kalman(&ekf[i],input[i]);
// 备注信息     
//-------------------------------------------------------------------------------------------------------------------
void Kalman(EKF_filter *ekf,float input)  //一阶卡尔曼滤波
{
	ekf->Now_P = ekf->LastP + ekf->Q;
	ekf->Kg = ekf->Now_P / (ekf->Now_P + ekf->R);
	ekf->out = ekf->out + ekf->Kg * (input - ekf->out);
	ekf->LastP = (1-ekf->Kg) * ekf->Now_P ;
}
                    /*******PID 相关********/
                    /*******PID 相关********/
                    /*******PID 相关********/

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     位置式 PID 运算函数
// 参数说明     SetValue：目标值；ActualValue：当前值；PID：PID 结构体指针
// 返回参数     位置式误差
// 使用示例     result = PID_Loc(setValue, currentValue, &pidStruct);
// 备注信息     进行位置式 PID 运算，计算误差、累计误差并进行积分限幅，最终得到位置式误差输出
//-------------------------------------------------------------------------------------------------------------------
float PID_Loc(float SetValue, float ActualValue, PID_Loc_str *PID)
{
    float PIDLoc;
    //误差计算
    PID->Ek = SetValue - ActualValue;
    //误差累计
    PID->LocSum += PID->Ek;
    //积分限幅
    if(PID->LocSum > PID->LIMT) PID->LocSum = PID->LIMT;
    if(PID->LocSum < -PID->LIMT) PID->LocSum = -PID->LIMT;
    //PID 计算
    PIDLoc =  (PID->Kp * PID->Ek)+//Kp
              (PID->Ki * PID->LocSum) +//Ki
              (PID->Kd * (PID->Ek - PID->Ek1));//Kd

    PID->Ek1 = PID->Ek;// 存储误差，用于下次计算
    return PIDLoc;
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     增量式 PID 运算函数
// 参数说明     CurrentPoint：当前值；SetPoint：目标值；PID：PID 结构体指针
// 返回参数     增量式误差
// 使用示例     result = PID_Inc(currentValue, setValue, &pidStruct);
// 备注信息     进行增量式 PID 运算，计算当前误差并更新历史误差，最终得到增量式误差输出
//------------------------------------------------------------------------------------------------------------------
float PID_Inc(float CurrentPoint,float SetPoint,PID_Inc_str* PID)
{
    PID->Error=SetPoint-CurrentPoint;                                     // 计算当前误差
         // KP
         PID->Pid_Out+=PID->Kp * (PID->Error-PID->Error1)
         // KI
         +PID->Ki * PID->Error
         // KD
         +PID->Kd * (PID->Error-2*PID->Error1+PID->Error2);

    PID->Error2=PID->Error1;     // 存储误差，用于下次计算
    PID->Error1=PID->Error;

    return (PID->Pid_Out);                                    // 返回增量值
}
//-------------------------------------------------------------------------------------------------------------------
// 函数简介     低通滤波器函数
// 参数说明     Data：当前值；Filter：低通滤波器 结构体指针
// 返回参数     滤波后的值
// 使用示例     result = Low_Pass_Filter(currentValue, &Filter);
// 备注信息     
//------------------------------------------------------------------------------------------------------------------
float Low_Pass_Filter(float Data,Pass_Str* Filter){
	Filter->Out=Filter->Out_Last+Filter->alpha*(Data-Filter->Out_Last);
	Filter->Out_Last=Filter->Out;
	return Filter->Out;
}

